/**
  ******************************************************************************
  * @file 	zmotors.c 
  * @brief  Protocol header file 
  ******************************************************************************
  */
  

#include "zbPWM.h"

// global for motors
TIM_OCInitTypeDef  			TIM_OCInitStructure;
TIM_TimeBaseInitTypeDef  	TIM_TimeBaseStructure;
u16 period = 0;

void initPWMOUT()
{ 	
	mygVars->PWMOUTFQ = 10000;
	// init all nesissary clocks
	RCC_APB1PeriphClockCmd(ZPWM0_TIM_CLK, ENABLE);
	RCC_APB1PeriphClockCmd(ZPWM0_TIM_CLK2, ENABLE);
	/* GPIOA and GPIOB clock enable */
	RCC_AHB1PeriphClockCmd(ZPWMO_GPIO_CLKS, ENABLE); 
	// init all motors gpio
	initPWMOUTGPIO();
	// fq
	setPWMOFrequency(mygVars->PWMOUTFQ);
	// duties
	setupPWMOUTDuties(&mygVars->PWMOUTCH1Duty, chAll);
}

void disablePWMOUT()
{
	//reset bits to stop motors
	initPWMOUTGPIO();
	TIM_CtrlPWMOutputs(ZPWM0_TIM, DISABLE);
	TIM_CtrlPWMOutputs(ZPWM0_TIM2, DISABLE);
	// stop timer and gpio clocks
	// cant do this because something else might be using that 
	RCC_APB1PeriphClockCmd( ZPWMO_CLK_TIM, DISABLE );

}

void updatePWMOUT(void)
{
	u8 channelsChanged = 0;
	u8 cont = false;
	
	// if there has been a change in the enabled state
	// of at least some perph
	if(mygVars->enabled != newgVars->enabled)
	{
		// if we were off 
		if(!(mygVars->enabled & enPWMOUT))
		{
			// and now we are on
			if(newgVars->enabled & enPWMOUT)
			{
				initPWMOUT();
				// update to know that we are on now
				mygVars->enabled = (mygVars->enabled | enPWMOUT);	
				cont = true;
			}
		}
		// if we were on
		if(mygVars->enabled & enPWMOUT)
		{
			// and now we are off
			if(!(newgVars->enabled & enPWMOUT))
			{
				disablePWMOUT();
				// clear bit so we are off next time
				mygVars->enabled = (mygVars->enabled & !enPWMOUT);
			}
		}
	}
	
	// if we get to this point and we are disabled just ret
	if(!(mygVars->enabled & enPWMOUT) && (cont == false))
	{
		return;
	}
	
	// check PWMOUT and update
	if(mygVars->PWMOUTFQ != newgVars->PWMOUTFQ)
	{
		// update the frequency in the old
		mygVars->PWMOUTFQ = newgVars->PWMOUTFQ;
		setPWMOFrequency(newgVars->PWMOUTFQ);
	}
	
	// check the duty cycles and update if needed
	if(*(u32*)(&mygVars->PWMOUTCH1Duty) != *(u32*)(&newgVars->PWMOUTCH1Duty))
	{
		if(mygVars->PWMOUTCH1Duty != newgVars->PWMOUTCH1Duty)
		{
			mygVars->PWMOUTCH1Duty = newgVars->PWMOUTCH1Duty;
			channelsChanged |= ch1;
		}
		if(mygVars->PWMOUTCH2Duty != newgVars->PWMOUTCH2Duty)
		{
			mygVars->PWMOUTCH2Duty = newgVars->PWMOUTCH2Duty;
			channelsChanged |= ch2;
		}
		if(mygVars->PWMOUTCH3Duty != newgVars->PWMOUTCH3Duty)
		{
			mygVars->PWMOUTCH3Duty = newgVars->PWMOUTCH3Duty;
			channelsChanged |= ch3;
		}
		if(mygVars->PWMOUTCH4Duty != newgVars->PWMOUTCH4Duty)
		{
			mygVars->PWMOUTCH4Duty = newgVars->PWMOUTCH4Duty;
			channelsChanged |= ch4;
		}
	
	}
	if(*(u32*)(&mygVars->PWMOUTCH5Duty) != *(u32*)(&newgVars->PWMOUTCH5Duty))
	{
		if(mygVars->PWMOUTCH5Duty != newgVars->PWMOUTCH5Duty)
		{
			mygVars->PWMOUTCH5Duty = newgVars->PWMOUTCH5Duty;
			channelsChanged |= ch5;
		}
		if(mygVars->PWMOUTCH6Duty != newgVars->PWMOUTCH6Duty)
		{
			mygVars->PWMOUTCH6Duty = newgVars->PWMOUTCH6Duty;
			channelsChanged |= ch6;
		}
		if(mygVars->PWMOUTCH7Duty != newgVars->PWMOUTCH7Duty)
		{
			mygVars->PWMOUTCH7Duty = newgVars->PWMOUTCH7Duty;
			channelsChanged |= ch7;
		}
		if(mygVars->PWMOUTCH8Duty != newgVars->PWMOUTCH8Duty)
		{
			mygVars->PWMOUTCH8Duty = newgVars->PWMOUTCH8Duty;
			channelsChanged |= ch8;
		}
	}

	if(channelsChanged != 0)
	{
		// send it to the unit, only changing changed pulse widths
		setupPWMOUTDuties(&mygVars->PWMOUTCH1Duty, channelsChanged);
	}
}


void initPWMOUTGPIO(void)
{
	GPIO_InitTypeDef  		GPIO_InitStructure;
	
	// set it up and init
	GPIO_InitStructure.GPIO_Pin = ZPWMO_PIN1 | ZPWMO_PIN2 ;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
	GPIO_Init(ZPWMO_PORT1, &GPIO_InitStructure); 
	
	GPIO_InitStructure.GPIO_Pin = ZPWMO_PIN3 | ZPWMO_PIN4 ;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
	GPIO_Init(ZPWMO_PORT2, &GPIO_InitStructure); 

	GPIO_InitStructure.GPIO_Pin = ZPWMO_PIN5 | ZPWMO_PIN6 ;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
	GPIO_Init(ZPWMO_PORT3, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin = ZPWMO_PIN7 | ZPWMO_PIN8 ;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
	GPIO_Init(ZPWMO_PORT4, &GPIO_InitStructure);

	  /* Connect TIM3 pins to AF2 */  
	GPIO_PinAFConfig(ZPWMO_PORT1, ZPWMO_PINSOURCE1, ZPWM0_AF);
	GPIO_PinAFConfig(ZPWMO_PORT1, ZPWMO_PINSOURCE2, ZPWM0_AF);
	GPIO_PinAFConfig(ZPWMO_PORT1, ZPWMO_PINSOURCE3, ZPWM0_AF);
	GPIO_PinAFConfig(ZPWMO_PORT2, ZPWMO_PINSOURCE4, ZPWM0_AF);
	// tim2
	GPIO_PinAFConfig(ZPWMO_PORT3, ZPWMO_PINSOURCE5, ZPWM0_AF2);
	GPIO_PinAFConfig(ZPWMO_PORT3, ZPWMO_PINSOURCE6, ZPWM0_AF2);
	GPIO_PinAFConfig(ZPWMO_PORT4, ZPWMO_PINSOURCE7, ZPWM0_AF2);
	GPIO_PinAFConfig(ZPWMO_PORT4, ZPWMO_PINSOURCE8, ZPWM0_AF2);  
}

/**
	this will setup all parts of the pwm

*/

void setPWMOFrequency(u16 frequency)
{
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;
	u16 PrescalerValue = 0;
	period = ZPWMO_PERIOD;
	/* Compute the prescaler value */
	PrescalerValue = (uint16_t) ((SystemCoreClock /2) / 28000000) - 1;
	
	/* Time base configuration */
	TIM_TimeBaseStructure.TIM_Period = period;
	TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	
	TIM_TimeBaseInit(ZPWM0_TIM, &TIM_TimeBaseStructure);
	
	/* PWM1 Mode configuration: Channel1 */
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	
	TIM_OC1Init(ZPWM0_TIM, &TIM_OCInitStructure);
	
	TIM_OC1PreloadConfig(ZPWM0_TIM, TIM_OCPreload_Enable);
	
	/* PWM1 Mode configuration: Channel2 */
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	
	TIM_OC2Init(ZPWM0_TIM, &TIM_OCInitStructure);
	
	TIM_OC2PreloadConfig(ZPWM0_TIM, TIM_OCPreload_Enable);
	
	/* PWM1 Mode configuration: Channel3 */
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	
	TIM_OC3Init(ZPWM0_TIM, &TIM_OCInitStructure);
	
	TIM_OC3PreloadConfig(ZPWM0_TIM, TIM_OCPreload_Enable);
	
	/* PWM1 Mode configuration: Channel4 */
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	
	TIM_OC4Init(ZPWM0_TIM, &TIM_OCInitStructure);
	
	TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
	
	TIM_ARRPreloadConfig(ZPWM0_TIM, ENABLE);
	
	/* TIM3 enable counter */
	TIM_Cmd(ZPWM0_TIM, ENABLE);

	// again for next clock 
		/* Time base configuration */
	TIM_TimeBaseStructure.TIM_Period = period;
	TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	
	TIM_TimeBaseInit(ZPWM0_TIM2, &TIM_TimeBaseStructure);
	
	/* PWM1 Mode configuration: Channel1 */
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	
	TIM_OC1Init(ZPWM0_TIM2, &TIM_OCInitStructure);
	
	TIM_OC1PreloadConfig(ZPWM0_TIM2, TIM_OCPreload_Enable);
	
	/* PWM1 Mode configuration: Channel2 */
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	
	TIM_OC2Init(ZPWM0_TIM2, &TIM_OCInitStructure);
	
	TIM_OC2PreloadConfig(ZPWM0_TIM2, TIM_OCPreload_Enable);
	
	/* PWM1 Mode configuration: Channel3 */
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	
	TIM_OC3Init(ZPWM0_TIM2, &TIM_OCInitStructure);
	
	TIM_OC3PreloadConfig(ZPWM0_TIM2, TIM_OCPreload_Enable);
	
	/* PWM1 Mode configuration: Channel4 */
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	
	TIM_OC4Init(ZPWM0_TIM2, &TIM_OCInitStructure);
	
	TIM_OC4PreloadConfig(ZPWM0_TIM2, TIM_OCPreload_Enable);
	
	TIM_ARRPreloadConfig(ZPWM0_TIM, ENABLE);
	
	/* TIM2 enable counter */
	TIM_Cmd(ZPWM0_TIM2, ENABLE);
}

void setupPWMOUTDuties(u8 duty[8], u8 channels)
{
	u8 i					= 0;
	u16 pulse				= 0;
	u8	lDuty				= 0;

	for(i=0; i<=7; i++)
	{
		if(channels & 1<<i)
		{
			// pick init function to call for channel
			lDuty = duty[i];
			pulse = dutyCycleToPulse(lDuty, period);
			switch(i)
			{
			case 0:
				ZPWMO_PIN1_CCR = pulse;
				break;
			case 1:
				ZPWMO_PIN2_CCR = pulse;
				break;
			case 2:
				ZPWMO_PIN3_CCR = pulse;
				break;
			case 3:
				ZPWMO_PIN4_CCR = pulse;
				break;
			case 4:	 // bit 4, ch_1 tim2
				ZPWMO_PIN5_CCR = pulse;
				break;
			case 5:
				ZPWMO_PIN6_CCR = pulse;
				break;
			case 6:	 // bit 4, ch_1 tim2
				ZPWMO_PIN7_CCR = pulse;
				break;
			case 7:
				ZPWMO_PIN8_CCR = pulse;
				break;
			default:
				continue;
			}
		}
	}
}





// UTILITIY AND CONVERSION FUNCTIONS 
u16 frequencyToPeriod(u16 frequency, u16 *prescalar)
{
	u16 orgfrequency = frequency; 
	u16	period		 = (u16)((SystemCoreClock/frequency) -1);	
	prescalar		 = 0;
	
	// if they are using a frequency low enough to merit a prescalar
	//period = (SystemCoreClock / fq ) - 1 ... ex 11 = 6mhz
	while(frequency < 1098)
	{
		prescalar++;
		frequency 	+= orgfrequency;
		period		= (u16)((SystemCoreClock/frequency) -1); 
	}
	return period;
}

// for now dutyC must be 0-100 which is 
u16 dutyCycleToPulse(u8 duty, u16 period)
{
	float percent = 0;
	if(duty > 0)
	{
		percent = (((float)duty)/255);

		return (percent*(period-1)); 
	}
	return 0;
}

void testPWMOUT(void)
{
	u8 delta = 5;
	u8 toggle = 0;
	u8 delay = 0;
	// enable the motors newgVars->enabled
	newgVars->PWMOUTFQ = ZPWM0_FQ;
	newgVars->enabled = newgVars->enabled | enPWMOUT;
	printf("PWM Out test \r\n");
	while(debugChar == 'p')
	{
	  Delay(delay++);
	  if(newgVars->PWMOUTCH1Duty == 0xFF)
	  {
		  toggle = 1;
		  printf("PWM Inverting, toggle 1 \r\n");
	  }
	  if(newgVars->PWMOUTCH1Duty == 0x0)
	  {
		  toggle = 0;
		  printf("PWM Inverting, toggle 0 \r\n");
	  }
	  if(toggle == 0)
	  {
		  newgVars->PWMOUTCH1Duty = newgVars->PWMOUTCH1Duty + delta;
		  newgVars->PWMOUTCH2Duty = newgVars->PWMOUTCH2Duty - delta;
		  newgVars->PWMOUTCH3Duty = newgVars->PWMOUTCH3Duty + delta;
		  newgVars->PWMOUTCH4Duty = newgVars->PWMOUTCH4Duty - delta;
		  newgVars->PWMOUTCH5Duty = newgVars->PWMOUTCH5Duty + delta;
		  newgVars->PWMOUTCH6Duty = newgVars->PWMOUTCH6Duty - delta;
		  newgVars->PWMOUTCH7Duty = newgVars->PWMOUTCH7Duty + delta;
		  newgVars->PWMOUTCH8Duty = newgVars->PWMOUTCH8Duty - delta;
	  }else{
		  newgVars->PWMOUTCH1Duty = newgVars->PWMOUTCH1Duty - delta;
		  newgVars->PWMOUTCH2Duty = newgVars->PWMOUTCH2Duty + delta;
		  newgVars->PWMOUTCH3Duty = newgVars->PWMOUTCH3Duty - delta;
		  newgVars->PWMOUTCH4Duty = newgVars->PWMOUTCH4Duty + delta;
		  newgVars->PWMOUTCH5Duty = newgVars->PWMOUTCH5Duty - delta;
		  newgVars->PWMOUTCH6Duty = newgVars->PWMOUTCH6Duty + delta;
		  newgVars->PWMOUTCH7Duty = newgVars->PWMOUTCH7Duty - delta;
		  newgVars->PWMOUTCH8Duty = newgVars->PWMOUTCH8Duty + delta;
	  }
	  updatePWMOUT();
	}
}


